function [ path ] = gen_path( )
%GEN_PATH Summary of this function goes here
%   Detailed explanation goes here

    start   = [-0.5, 0.1, 0.2];
    finish  = [0.1, 0.3, 0.2];
    L1      = 0.4;
    L2      = 0.4;
    path    = zeros(10, 3);
    cart    = zeros(10, 3);
    
    cart(1:28, :) = repmat([-0.5 0.1 0.19], 28, 1);
    path(1:28, :) = repmat([118.2866 100.8069 0.19], 28, 1);
    cart(29:29, :) = repmat([-0.5 0.1 0.2], 1, 1);
    path(29:29, :) = repmat([118.2866 100.8069 0.2], 1, 1);
    
    steps = 29;
    for t = 0:1:steps
        i = 29 + floor((t) + 1);
        target = (1 - (t / steps)) .* start + ((t / steps) .* finish);
        x = target(1);
        y = target(2);
        z = target(3);
        
        cart(i, :) = [x y z];
        
        k = sqrt((x^2 + y^2 + L1^2 + L2^2)^2 - 2*((x^2 + y^2)^2 + L1^4 + L2^4));
        path(i, 1) = (180 / pi) * (atan2(y, x) - atan2(k, x^2 + y^2 + L1^2 - L2^2));
        path(i, 2) = (180 / pi) * (atan2(k, x^2 + y^2 - L1^2 - L2^2));
        path(i, 3) = z;
    end
    
    figure(1)
    
    time = linspace(0, length(path) / 10, length(path));
    
    % plot theta1, theta2 and L3
    subplot(2, 3, 1), plot(time, path(:, 1)), title('\Theta_1'), ylim([-180 180])
    subplot(2, 3, 2), plot(time, path(:, 2)), title('\Theta_2'), ylim([-45 180])
    subplot(2, 3, 3), plot(time, path(:, 3)), title('L_3'), ylim([0 0.25])
    
    % plot x, y and z
    subplot(2, 3, 4), plot(time, cart(:, 1)), title('x'), ylim([-0.6 0.2])
    subplot(2, 3, 5), plot(time, cart(:, 2)), title('y'), ylim([0.0 0.4])
    subplot(2, 3, 6), plot(time, cart(:, 3)), title('z'), ylim([0 0.25])
    
    figure(2)
end
